#pragma once

#include <Eigen/CholmodSupport>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <utility>

#include <DPGO/RelativePoseMeasurement.h>

namespace DChordal {

/** Some useful typedefs for the MM-Chordal library */
typedef double Scalar;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
typedef Eigen::Matrix<Scalar, 1, Eigen::Dynamic> RowVector;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic> DiagonalMatrix;

/** We use row-major storage order to take advantage of fast (sparse-matrix) *
 * (dense-vector) multiplications when OpenMP is available (cf. the Eigen
 * documentation page on "Eigen and Multithreading") */
typedef Eigen::SparseMatrix<Scalar, Eigen::RowMajor> SparseMatrix;

/** The type of the sparse Cholesky factorization to use in the computation of
 * the orthogonal projection operation */
typedef Eigen::CholmodDecomposition<SparseMatrix> SparseCholeskyFactorization;

/** The type of the incomplete Cholesky decomposition we will use for
 * preconditioning the conjugate gradient iterations in the RTR method */
typedef Eigen::IncompleteCholesky<Scalar> IncompleteCholeskyFactorization;

/** The loss kernels to formulate inter-node cost*/
enum class Loss {
  /** Trivial squared loss */
  None,
  /** Huber loss */
  // Huber
};

typedef DPGO::Index Index;
typedef DPGO::RelativePoseMeasurement RelativePoseMeasurement;
typedef DPGO::measurements_t measurements_t;

struct Options {
  /** Maximum permitted number of (outer) iterations of the Riemannian
   * trust-region method */
  int max_iterations = 1000;

  /** Maximum elapsed computation time (in seconds) */
  double max_computation_time = std::numeric_limits<double>::max();

  /** Regularized term for majorization minimization operator */
  double reg_G = 1e-12;

  /** The loss kernel */
  Loss loss = Loss::None;

  /** The loss parameter (only used for non-trivial losses) */
  double loss_reg = 1.0;

  /** Whether to print output as the algorithm runs */
  bool verbose = false;

  /** If this value is true, the DChordalReduced algorithm will log and return
   * the entire sequence of iterates generated by the Riemannian Staircase */
  bool log_iterates = false;
};

/** This struct contains the output of the DChordalReduced algorithm */
struct Result {
  bool updated = true;

  /** The current estimate of X */
  Matrix Xk;

  /** The current estimate of X^a */
  Matrix Xak;

  /** The norm of the gradient */
  Scalar gradFnorm;

  /** The estimate of Rhat at each iteration*/
  std::vector<Matrix> X;
  /** g at each iteration */
  std::vector<Matrix> g;

  /** The total number of iterations */
  int iters;

  /** The elapsed computation time of each iteration */
  std::vector<double> elapsed_optimization_times;

  /** Nesterov coefficients */
  std::vector<double> s;

  /** A vector containing the sequence of norms of the rotational gradients
   * obtained during the optimization at each iteration */
  std::vector<Scalar> gnorms;

  /** If DChordalReduced was run with log_iterates = true, this will contain the
   * sequence of iterates generated by Nesterov's method */
  std::vector<Matrix> iterates;

  void clear() {
    iters = 0;
    updated = true;
    X.clear();
    g.clear();
    s.clear();
    gnorms.clear();
    elapsed_optimization_times.clear();
    iterates.clear();
  }
};
}  // namespace DChordal

